#ifndef __MATRIX__
#define __MATRIX__

#include <iostream>
#include <cmath>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

#include <math/vector.h>

#define assignMatrix(M, u0, u4, u8, u12, u1, u5, u9, u13, u2, u6, u10, u14, u3, u7, u11, u15) { \
	M[0] = u0; M[4] = u4; M[ 8] =  u8; M[12] = u12; \
	M[1] = u1; M[5] = u5; M[ 9] =  u9; M[13] = u13; \
	M[2] = u2; M[6] = u6; M[10] = u10; M[14] = u14; \
	M[3] = u3; M[7] = u7; M[11] = u11; M[15] = u15; \
	}

namespace math {
	
template<class T>
class Mat4x4 {
	public:
		Mat4x4();
		Mat4x4(T[16]);
		Mat4x4(T, T, T, T, 
		       T, T, T, T, 
		       T, T, T, T, 
		       T, T, T, T);
		template<class U>
		explicit Mat4x4(const Mat4x4<U>& N) {
			for(size_t i = 0; i < 16; i++)
				M[i] = static_cast<T>(N[i]);
		}

		Mat4x4(const Mat4x4& m);
		
		Mat4x4& operator=(T[16]);

		Mat4x4& operator+=(Mat4x4&);
		Mat4x4& operator+=(T);

		Mat4x4& operator-=(Mat4x4);
		Mat4x4& operator-=(T);

		Mat4x4& operator*=(T);
		Mat4x4& operator*=(Mat4x4&);
		Mat4x4& operator*=(T[16]);
		
		Mat4x4& operator/=(T);

		T& operator[](int index) {return M[index];}
		const T& operator[](int index) const {return M[index];}

		Mat4x4& Identity();
		Mat4x4& RotateX(T a);
		Mat4x4& RotateY(T a);
		Mat4x4& RotateZ(T a);
		Mat4x4& Rotate(T a, const Vec<T, 3>& v);
		Mat4x4& Translate(T x, T y, T z);
		Mat4x4& Translate(const Vec<T, 3>& v);
		Mat4x4& Scale(T x, T y, T z);
		Mat4x4& Scale(const Vec<T, 3>& v);
		Mat4x4& Inverse();
		Mat4x4& Transpose();
		Mat4x4& Rotate(const Vec<T, 3>& v);

		void Set(T u0, T u4, T  u8, T u12, 
			 T u1, T u5, T  u9, T u13, 
			 T u2, T u6, T u10, T u14, 
			 T u3, T u7, T u11, T u15);

		Mat4x4& Frustum(T l, T r, T b, T t, T n, T f);
		Mat4x4& Perspective(T fovy, T aspect, T n, T f);
		Mat4x4& Ortho(T l, T r, T b, T t, T n, T f);
		Mat4x4& Lookat(T posx, T posy, T posz, 
			       T centerx, T centery, T centerz,	
			       T upx, T upy, T upz);
		const T* GetData() const {return M;}
	private:
		T M[16];
		bool isIdentity;
		
};

template<class T>
Mat4x4<T>& Rotate(const Vec<T, 3>& v) {
	Vec<T, 3> v2 = v;
	normalize(v2);
	Vec<T, 3> y = V3n(0.0, 0.0, 1.0);
	T theta = acos(dot(v2, y));
	theta = (180.0 * theta) / M_PI;

	Vec<T, 3> c = cross(y, v2);
	normalize(c);

	return Rotate(-theta, c);
}

template<class T>
Mat4x4<T>::Mat4x4() : isIdentity(true) {
	Identity();
}

template<class T>
Mat4x4<T>::Mat4x4(T M[16]) : isIdentity(false) {
	for(int i = 0; i < 16; i++)
		this->M[i] = M[i];
}

template<class T>
Mat4x4<T>::Mat4x4(const Mat4x4& m) {
	for(int i = 0; i < 16; i++)
		M[i] = m.M[i];
	isIdentity = false;
}

template<class T>
Mat4x4<T>::Mat4x4(T u0, T u4, T  u8, T u12, 
		 T u1, T u5, T  u9, T u13, 
		 T u2, T u6, T u10, T u14, 
		 T u3, T u7, T u11, T u15) : isIdentity(false) {
	assignMatrix(M, u0, u4, u8, u12,
   			u1, u5, u9, u13,
			u2, u6, u10, u14,
			u3, u7, u11, u15);
}


template<class T>
Mat4x4<T>& Mat4x4<T>::Lookat(T posx, T posy, T posz, 
					  T centerx, T centery, T centerz, 
					  T upx, T upy, T upz) {
	T N[16];
	Vec<T, 3> u;
	u.Set(upx, upy, upz);

	Vec<T, 3> f;
	f.Set(centerx - posx, centery - posy, centerz - posz);

	normalize(f);
	Vec<T, 3> s = cross(f, u);

	normalize(s);
	u = cross(s,f);

	assignMatrix(N,  s[0],  s[1],  s[2], -s[0] * posx - s[1] * posy - s[2] * posz,
			 u[0],  u[1],  u[2], -u[0] * posx - u[1] * posy - u[2] * posz,
			-f[0], -f[1], -f[2],  f[0] * posx + f[1] * posy + f[2] * posz,
			 0.0,  0.0,  0.0, 1.0);	
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator+=(Mat4x4& N) {
	for(int i = 0; i < 16; i++)
		M[i] += N.M[i];
	isIdentity = false;
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator+=(T a) {
	for(int i = 0; i < 16; i++)
		M[i] += a;
	isIdentity = false;
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator-=(Mat4x4 N) {
	for(int i = 0; i < 16; i++)
		M[i] -= N.M[i];
	isIdentity = false;
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator-=(T a) {
	for(int i = 0; i < 16; i++)
		M[i] -= a;
	isIdentity = false;
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator*=(T a) {
	for(int i = 0; i < 16; i++)
		M[i] *= a;
	isIdentity = false;
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator*=(Mat4x4& N) {
	T U[16];
	U[ 0] = M[0] * N.M[ 0] + M[4] * N.M[ 1] + M[ 8] * N.M[ 2] + M[12] * N.M[ 3];
	U[ 1] = M[1] * N.M[ 0] + M[5] * N.M[ 1] + M[ 9] * N.M[ 2] + M[13] * N.M[ 3];
	U[ 2] = M[2] * N.M[ 0] + M[6] * N.M[ 1] + M[10] * N.M[ 2] + M[14] * N.M[ 3];
	U[ 3] = M[3] * N.M[ 0] + M[7] * N.M[ 1] + M[11] * N.M[ 2] + M[15] * N.M[ 3];

	U[ 4] = M[0] * N.M[ 4] + M[4] * N.M[ 5] + M[ 8] * N.M[ 6] + M[12] * N.M[ 7];
	U[ 5] = M[1] * N.M[ 4] + M[5] * N.M[ 5] + M[ 9] * N.M[ 6] + M[13] * N.M[ 7];
	U[ 6] = M[2] * N.M[ 4] + M[6] * N.M[ 5] + M[10] * N.M[ 6] + M[14] * N.M[ 7];
	U[ 7] = M[3] * N.M[ 4] + M[7] * N.M[ 5] + M[11] * N.M[ 6] + M[15] * N.M[ 7];

	U[ 8] = M[0] * N.M[ 8] + M[4] * N.M[ 9] + M[ 8] * N.M[10] + M[12] * N.M[11];
	U[ 9] = M[1] * N.M[ 8] + M[5] * N.M[ 9] + M[ 9] * N.M[10] + M[13] * N.M[11];
	U[10] = M[2] * N.M[ 8] + M[6] * N.M[ 9] + M[10] * N.M[10] + M[14] * N.M[11];
	U[11] = M[3] * N.M[ 8] + M[7] * N.M[ 9] + M[11] * N.M[10] + M[15] * N.M[11];

	U[12] = M[0] * N.M[12] + M[4] * N.M[13] + M[ 8] * N.M[14] + M[12] * N.M[15];
	U[13] = M[1] * N.M[12] + M[5] * N.M[13] + M[ 9] * N.M[14] + M[13] * N.M[15];
	U[14] = M[2] * N.M[12] + M[6] * N.M[13] + M[10] * N.M[14] + M[14] * N.M[15];
	U[15] = M[3] * N.M[12] + M[7] * N.M[13] + M[11] * N.M[14] + M[15] * N.M[15];

	this->operator =(U);
	isIdentity = false;

	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator*=(T N[16]) {
	T U[16];
	U[ 0] = M[0] * N[ 0] + M[4] * N[ 1] + M[ 8] * N[ 2] + M[12] * N[ 3];
	U[ 1] = M[1] * N[ 0] + M[5] * N[ 1] + M[ 9] * N[ 2] + M[13] * N[ 3];
	U[ 2] = M[2] * N[ 0] + M[6] * N[ 1] + M[10] * N[ 2] + M[14] * N[ 3];
	U[ 3] = M[3] * N[ 0] + M[7] * N[ 1] + M[11] * N[ 2] + M[15] * N[ 3];

	U[ 4] = M[0] * N[ 4] + M[4] * N[ 5] + M[ 8] * N[ 6] + M[12] * N[ 7];
	U[ 5] = M[1] * N[ 4] + M[5] * N[ 5] + M[ 9] * N[ 6] + M[13] * N[ 7];
	U[ 6] = M[2] * N[ 4] + M[6] * N[ 5] + M[10] * N[ 6] + M[14] * N[ 7];
	U[ 7] = M[3] * N[ 4] + M[7] * N[ 5] + M[11] * N[ 6] + M[15] * N[ 7];

	U[ 8] = M[0] * N[ 8] + M[4] * N[ 9] + M[ 8] * N[10] + M[12] * N[11];
	U[ 9] = M[1] * N[ 8] + M[5] * N[ 9] + M[ 9] * N[10] + M[13] * N[11];
	U[10] = M[2] * N[ 8] + M[6] * N[ 9] + M[10] * N[10] + M[14] * N[11];
	U[11] = M[3] * N[ 8] + M[7] * N[ 9] + M[11] * N[10] + M[15] * N[11];

	U[12] = M[0] * N[12] + M[4] * N[13] + M[ 8] * N[14] + M[12] * N[15];
	U[13] = M[1] * N[12] + M[5] * N[13] + M[ 9] * N[14] + M[13] * N[15];
	U[14] = M[2] * N[12] + M[6] * N[13] + M[10] * N[14] + M[14] * N[15];
	U[15] = M[3] * N[12] + M[7] * N[13] + M[11] * N[14] + M[15] * N[15];

	this->operator =(U);
	isIdentity = false;

	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator/=(T a) {
	for(int i = 0; i < 16; i++)
		M[i] /= a;
	isIdentity = false;
	return *this;
}

template<class T>
void Mat4x4<T>::Set(T u0, T u4, T  u8, T u12, 
					T u1, T u5, T  u9, T u13, 
					T u2, T u6, T u10, T u14, 
					T u3, T u7, T u11, T u15) {
	assignMatrix(M, u0, u4,  u8, u12,
			u1, u5,  u9, u13,
			u2, u6, u10, u14,
			u3, u7, u11, u15);
}

template<class T>
std::ostream& operator<<(std::ostream& os, Mat4x4<T>& N) {
	os	<< N[0] << " " << N[4] << " " << N[8] << " " << N[12] << std::endl
		<< N[1] << " " << N[5] << " " << N[9] << " " << N[13] << std::endl
		<< N[2] << " " << N[6] << " " << N[10] << " " << N[14] << std::endl
		<< N[3] << " " << N[7] << " " << N[11] << " " << N[15];

	return os;
}

template<class T>
Vec<T, 4> operator*(const Mat4x4<T>& N, const Vec<T, 4>& v) {
	Vec<T, 4> u;
	u[0] = N[0] * v[0] + N[4] * v[1] + N[ 8] * v[2] + N[12] * v[3];
	u[1] = N[1] * v[0] + N[5] * v[1] + N[ 9] * v[2] + N[13] * v[3];
	u[2] = N[2] * v[0] + N[6] * v[1] + N[10] * v[2] + N[14] * v[3];
	u[3] = N[3] * v[0] + N[7] * v[1] + N[11] * v[2] + N[15] * v[3];
	return u;
}

template<class T>
Mat4x4<T> operator*(Mat4x4<T>& M, Mat4x4<T>& N) {
	T U[16];
	U[ 0] = M[0] * N[ 0] + M[4] * N[ 1] + M[ 8] * N[ 2] + M[12] * N[ 3];
	U[ 1] = M[1] * N[ 0] + M[5] * N[ 1] + M[ 9] * N[ 2] + M[13] * N[ 3];
	U[ 2] = M[2] * N[ 0] + M[6] * N[ 1] + M[10] * N[ 2] + M[14] * N[ 3];
	U[ 3] = M[3] * N[ 0] + M[7] * N[ 1] + M[11] * N[ 2] + M[15] * N[ 3];

	U[ 4] = M[0] * N[ 4] + M[4] * N[ 5] + M[ 8] * N[ 6] + M[12] * N[ 7];
	U[ 5] = M[1] * N[ 4] + M[5] * N[ 5] + M[ 9] * N[ 6] + M[13] * N[ 7];
	U[ 6] = M[2] * N[ 4] + M[6] * N[ 5] + M[10] * N[ 6] + M[14] * N[ 7];
	U[ 7] = M[3] * N[ 4] + M[7] * N[ 5] + M[11] * N[ 6] + M[15] * N[ 7];

	U[ 8] = M[0] * N[ 8] + M[4] * N[ 9] + M[ 8] * N[10] + M[12] * N[11];
	U[ 9] = M[1] * N[ 8] + M[5] * N[ 9] + M[ 9] * N[10] + M[13] * N[11];
	U[10] = M[2] * N[ 8] + M[6] * N[ 9] + M[10] * N[10] + M[14] * N[11];
	U[11] = M[3] * N[ 8] + M[7] * N[ 9] + M[11] * N[10] + M[15] * N[11];

	U[12] = M[0] * N[12] + M[4] * N[13] + M[ 8] * N[14] + M[12] * N[15];
	U[13] = M[1] * N[12] + M[5] * N[13] + M[ 9] * N[14] + M[13] * N[15];
	U[14] = M[2] * N[12] + M[6] * N[13] + M[10] * N[14] + M[14] * N[15];
	U[15] = M[3] * N[12] + M[7] * N[13] + M[11] * N[14] + M[15] * N[15];
	return Mat4x4<T>(U);
}

template<class T>
Mat4x4<T> operator*(Mat4x4<T>& N, T a) {
	return Mat4x4<T>(N[0] * a, N[4] * a, N[ 8] * a, N[12] * a,
		      	 N[1] * a, N[5] * a, N[ 9] * a, N[13] * a,
		      	 N[2] * a, N[6] * a, N[10] * a, N[14] * a,
			 N[3] * a, N[7] * a, N[11] * a, N[15] * a);
}

template<class T>
Mat4x4<T> operator*(T a, Mat4x4<T>& N) {
	return Mat4x4<T>(N[0] * a, N[4] * a, N[ 8] * a, N[12] * a,
			 N[1] * a, N[5] * a, N[ 9] * a, N[13] * a,
			 N[2] * a, N[6] * a, N[10] * a, N[14] * a,
			 N[3] * a, N[7] * a, N[11] * a, N[15] * a);
}

template<class T>
Mat4x4<T> operator/(Mat4x4<T>& N, T a) {
	T b = 1.0 / a;
	return Mat4x4<T>(N[0] * b, N[4] * b, N[ 8] * b, N[12] * b,
			 N[1] * b, N[5] * b, N[ 9] * b, N[13] * b,
			 N[2] * b, N[6] * b, N[10] * b, N[14] * b,
			 N[3] * b, N[7] * b, N[11] * b, N[15] * b);
}

template<class T>
Mat4x4<T> operator/(T a, Mat4x4<T>& N) {
	T b = 1.0 / a;
	return Mat4x4<T>(N[0] * b, N[4] * b, N[ 8] * b, N[12] * b,
			 N[1] * b, N[5] * b, N[ 9] * b, N[13] * b,
			 N[2] * b, N[6] * b, N[10] * b, N[14] * b,
			 N[3] * b, N[7] * b, N[11] * b, N[15] * b);
}

template<class T>
Mat4x4<T> translate(T x, T y, T z) {
	return Mat4x4<T>(1.0, 0.0, 0.0, x,
			 0.0, 1.0, 0.0, y,
			 0.0, 0.0, 1.0, z,
			 0.0, 0.0, 0.0, 1.0);
}

template<class T>
Mat4x4<T> scale(T x, T y, T z) {
	return Mat4x4<T>(x, 0.0, 0.0, 0.0, 
		  	 0.0, y, 0.0, 0.0,
			 0.0, 0.0, z, 0.0,
			 0.0, 0.0, 0.0, 1.0);
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Identity() {
	assignMatrix(M, 1.0, 0.0, 0.0, 0.0,
			0.0, 1.0, 0.0, 0.0,
			0.0, 0.0, 1.0, 0.0,
			0.0, 0.0, 0.0, 1.0);
	isIdentity = true;
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::RotateX(T x) {
	T a = (M_PI * (x)) / 180.0, N[16];
	assignMatrix(N, 1.0, 0.0, 0.0, 0.0,
			0.0, std::cos(a), -std::sin(a), 0.0,
			0.0, std::sin(a), std::cos(a), 0.0,
			0.0, 0.0, 0.0f, 1.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::RotateY(T x) {
	T a = (M_PI * (x)) / 180.0, N[16];
	assignMatrix(N, std::cos(a), 0.0, std::sin(a), 0.0,
			0.0, 1.0, 0.0, 0.0,
			-std::sin(a), 0.0, std::cos(a), 0.0,
			0.0, 0.0, 0.0, 1.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::RotateZ(T x) {
	T a = (M_PI * (x)) / 180.0, N[16];
	assignMatrix(N, std::cos(a), -std::sin(a), 0.0, 0.0,
			std::sin(a), std::cos(a), 0.0, 0.0,
			0.0, 0.0, 1.0, 0.0,
			0.0, 0.0, 0.0, 1.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}
		
template<class T>
Mat4x4<T>& Mat4x4<T>::Rotate(T a, const Vec<T, 3>& v) {
	T theta = (M_PI * (a)) / 180.0, N[16];

	T c = std::cos(theta);
	T s = std::sin(theta);
	T t = (T)1 - c; 

	const T& x = v[0];
	const T& y = v[1];
	const T& z = v[2];

	assignMatrix(N, t * x * x + c, t * x * y + s * z, t * x * z - s * y, 0.0,
					t * x * y - s * z, t * y * y + c, t * y * z + s * x, 0.0,
					t * x * z + s * y, t * y * z - s * x, t * z * z + c, 0.0,
					0.0, 0.0, 0.0, 1.0);
	
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Translate(T x, T y, T z) {
	T N[16];
	assignMatrix(N, 1.0, 0.0, 0.0, x,
			0.0, 1.0, 0.0, y,
			0.0, 0.0, 1.0, z,
			0.0, 0.0, 0.0, 1.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Translate(const math::Vec<T, 3>& v) {
	return Translate(v[0], v[1], v[2]);
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Scale(T x, T y, T z) {
	T N[16];
	assignMatrix(N, x, 0.0, 0.0, 0.0,
			0.0, y, 0.0, 0.0,
			0.0, 0.0, z, 0.0,
			0.0, 0.0, 0.0, 1.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Scale(const math::Vec<T, 3>& v) {
	return Scale(v[0], v[1], v[2]);
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Inverse() {
	T N[16];
	
	T x = 1.0f / (M[3] * M[6] * M[9] * M[12] - M[2] * M[7] * M[9] * M[12] - M[3] * M[5] * M[10] * M[12] + M[1] * M[7] * M[10] * M[12] + M[2] * M[5] * M[11] * M[12] - M[1] * M[6] * M[11] * M[12] - M[3] * M[6] * M[8] * M[13] + M[2] * M[7] * M[8] * M[13] + M[3] * M[4] * M[10] * M[13] - M[0] * M[7] * M[10] * M[13] - M[2] * M[4] * M[11] * M[13] + M[0] * M[6] * M[11] * M[13] + M[3] * M[5] * M[8] * M[14] - M[1] * M[7] * M[8] * M[14] - M[3] * M[4] * M[9] * M[14] + M[0] * M[7] * M[9] * M[14] + M[1] * M[4] * M[11] * M[14] - M[0] * M[5] * M[11] * M[14] - M[2] * M[5] * M[8] * M[15] + M[1] * M[6] * M[8] * M[15] + M[2] * M[4] * M[9] * M[15] - M[0] * M[6] * M[9] * M[15] - M[1] * M[4] * M[10] * M[15] + M[0] * M[5] * M[10] * M[15]);
	N[ 0] = (-M[7] * M[10] * M[13] + M[6] * M[11] * M[13] + M[7] * M[9] * M[14] - M[5] * M[11] * M[14] - M[6] * M[9] * M[15] + M[5] * M[10] * M[15]) * x;
	N[ 4] = ( M[7] * M[10] * M[12] - M[6] * M[11] * M[12] - M[7] * M[8] * M[14] + M[4] * M[11] * M[14] + M[6] * M[8] * M[15] - M[4] * M[10] * M[15]) * x;
	N[ 8] = (-M[7] * M[ 9] * M[12] + M[5] * M[11] * M[12] + M[7] * M[8] * M[13] - M[4] * M[11] * M[13] - M[5] * M[8] * M[15] + M[4] * M[ 9] * M[15]) * x;
	N[12] = ( M[6] * M[ 9] * M[12] - M[5] * M[10] * M[12] - M[6] * M[8] * M[13] + M[4] * M[10] * M[13] + M[5] * M[8] * M[14] - M[4] * M[ 9] * M[14]) * x;

	N[ 1] = ( M[3] * M[10] * M[13] - M[2] * M[11] * M[13] - M[3] * M[9] * M[14] + M[1] * M[11] * M[14] + M[2] * M[9] * M[15] - M[1] * M[10] * M[15]) * x;
	N[ 5] = (-M[3] * M[10] * M[12] + M[2] * M[11] * M[12] + M[3] * M[8] * M[14] - M[0] * M[11] * M[14] - M[2] * M[8] * M[15] + M[0] * M[10] * M[15]) * x;
	N[ 9] = ( M[3] * M[ 9] * M[12] - M[1] * M[11] * M[12] - M[3] * M[8] * M[13] + M[0] * M[11] * M[13] + M[1] * M[8] * M[15] - M[0] * M[ 9] * M[15]) * x;
	N[13] = (-M[2] * M[ 9] * M[12] + M[1] * M[10] * M[12] + M[2] * M[8] * M[13] - M[0] * M[10] * M[13] - M[1] * M[8] * M[14] + M[0] * M[ 9] * M[14]) * x;

	N[ 2] = (-M[3] * M[ 6] * M[13] + M[2] * M[ 7] * M[13] + M[3] * M[5] * M[14] - M[1] * M[ 7] * M[14] - M[2] * M[5] * M[15] + M[1] * M[ 6] * M[15]) * x;
	N[ 6] = ( M[3] * M[ 6] * M[12] - M[2] * M[ 7] * M[12] - M[3] * M[4] * M[14] + M[0] * M[ 7] * M[14] + M[2] * M[4] * M[15] - M[0] * M[ 6] * M[15]) * x;
	N[10] = (-M[3] * M[ 5] * M[12] + M[1] * M[ 7] * M[12] + M[3] * M[4] * M[13] - M[0] * M[ 7] * M[13] - M[1] * M[4] * M[15] + M[0] * M[ 5] * M[15]) * x;
	N[14] = ( M[2] * M[ 5] * M[12] - M[1] * M[ 6] * M[12] - M[2] * M[4] * M[13] + M[0] * M[ 6] * M[13] + M[1] * M[4] * M[14] - M[0] * M[ 5] * M[14]) * x;

	N[ 3] = ( M[3] * M[ 6] * M[ 9] - M[2] * M[ 7] * M[ 9] - M[3] * M[5] * M[10] + M[1] * M[ 7] * M[10] + M[2] * M[5] * M[11] - M[1] * M[ 6] * M[11]) * x;
	N[ 7] = (-M[3] * M[ 6] * M[ 8] + M[2] * M[ 7] * M[ 8] + M[3] * M[4] * M[10] - M[0] * M[ 7] * M[10] - M[2] * M[4] * M[11] + M[0] * M[ 6] * M[11]) * x;
	N[11] = ( M[3] * M[ 5] * M[ 8] - M[1] * M[ 7] * M[ 8] - M[3] * M[4] * M[ 9] + M[0] * M[ 7] * M[ 9] + M[1] * M[4] * M[11] - M[0] * M[ 5] * M[11]) * x;
	N[15] = (-M[2] * M[ 5] * M[ 8] + M[1] * M[ 6] * M[ 8] + M[2] * M[4] * M[ 9] - M[0] * M[ 6] * M[ 9] - M[1] * M[4] * M[10] + M[0] * M[ 5] * M[10]) * x;
	
	//this->operator=(N);
	for(int i = 0; i < 16; i++)
		M[i] = N[i];
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Frustum(T l, T r, T b, T t, T n, T f) {
	T N[16];

	assignMatrix(N, 2.0 * n / (r - l), 0.0, (r + l) / (r - l), 0.0,
			0.0, (2.0 * n) / (t - b), (t + b) / (t - b), 0.0,
			0.0, 0.0, -(f + n) / (f - n), (-2.0 * f * n) / (f - n),
			0.0, 0.0, -1.0, 0.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Perspective(T fovy, T aspect, T n, T f) {
	T xmin, xmax, ymin, ymax;
	ymax = n * std::tan(fovy * M_PI / 360.0);
	ymin = -ymax;
	xmin = ymin * aspect;
	xmax = ymax * aspect;
	return Frustum(xmin, xmax, ymin, ymax, n, f);
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Ortho(T l, T r, T b, T t, T n, T f) {
	T N[16];

	assignMatrix(N, 2.0 / (r - l), 0.0, 0.0, (r + l) / (r - l),
			0.0, 2.0 / (t - b), 0.0, (t + b) / (t - b),
			0.0, 0.0, -2.0 / (f - n), (f + n) / (f - n),
			0.0, 0.0, 0.0, 1.0);
	if(isIdentity)
		this->operator=(N);
	else
		this->operator*=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::Transpose() {
	T N[16];

	assignMatrix(N, M[0], M[1], M[2], M[3], 
			M[4], M[5], M[6], M[7], 
			M[8], M[9], M[10], M[11], 
			M[12], M[13], M[14], M[15]);
	this->operator=(N);
	return *this;
}

template<class T>
Mat4x4<T>& Mat4x4<T>::operator=(T N[16]) {
	M[0] = N[0]; M[4] = N[4]; M[ 8] = N[ 8]; M[12] = N[12];
	M[1] = N[1]; M[5] = N[5]; M[ 9] = N[ 9]; M[13] = N[13];
	M[2] = N[2]; M[6] = N[6]; M[10] = N[10]; M[14] = N[14];
	M[3] = N[3]; M[7] = N[7]; M[11] = N[11]; M[15] = N[15];
	isIdentity = false;

	return *this;
}
typedef Mat4x4<float> Mat4x4f;

}

#endif //__MATRIX__
